function [periodMsgParams, totalPacketCount, totalMsgCount, retCode] = parsegsof( rx, endMsgName ) % TODO: 代码生成支持
%PARSEGSOF 解析GSOF协议格式数据并将解析后的数据保存至文件中
% rx: uint8向量，待解析的内容
% endMsgName: 字符串，每个周期最后一个消息的类型名
% retCode: 标量，0表示无错误，-1表示校验错误

persistent p_PGSOF_totalRx p_PGSOF_totalMsg p_PGSOF_totalPacketItem p_PGSOF_totalPacketCount p_PGSOF_totalMsgCount p_PGSOF_periodMsgParams

% 初始化
if isempty(p_PGSOF_totalPacketCount)
    p_PGSOF_totalPacketCount = 0;
end
if isempty(p_PGSOF_totalMsgCount)
    p_PGSOF_totalMsgCount = 0;
end

periodMsgParams = struct([]);
retCode = 0;
p_PGSOF_totalRx = [p_PGSOF_totalRx, rx(:)'];
% 进行包解析，提取并合并消息数据
while true
    [p_PGSOF_totalRx, msg, packetItem, hasValidPacket, parseSuccess] = parsepacket(p_PGSOF_totalRx); % 解析一个包
    if ~parseSuccess
        retCode = -1;
    end
    if hasValidPacket
        p_PGSOF_totalPacketCount = p_PGSOF_totalPacketCount + 1;
        p_PGSOF_totalMsg = [p_PGSOF_totalMsg, msg]; %#ok<AGROW>
        p_PGSOF_totalPacketItem = [p_PGSOF_totalPacketItem, packetItem]; %#ok<AGROW>
    else
        break;
    end
end
% 进行消息解析
while true
    [msgParams, p_PGSOF_totalMsg, p_PGSOF_totalPacketItem, hasValidMsg] = parsemsg(p_PGSOF_totalMsg, p_PGSOF_totalPacketItem); % 解析一条消息
    if hasValidMsg
        p_PGSOF_totalMsgCount = p_PGSOF_totalMsgCount + 1;
        names = fieldnames(msgParams);
        p_PGSOF_periodMsgParams.(names{1}) = msgParams.(names{1});
        if strcmpi(names{1}, endMsgName)
            p_PGSOF_periodMsgParams.isComplete = struct('isComplete',1);
            periodMsgParams = p_PGSOF_periodMsgParams;
            p_PGSOF_periodMsgParams = struct();
            break;
        end
    else
        break;
    end
end
totalPacketCount = p_PGSOF_totalPacketCount;
totalMsgCount = p_PGSOF_totalMsgCount;
remainingMsgLen = length(p_PGSOF_totalMsg);
end

function [rx, msg, packetItem, hasValidPacket, success] = parsepacket(rx)
msg = uint8([]);
packetItem = zeros(4, 0, 'uint8'); % 各行依次为status、transmission number、page index、max page index
hasValidPacket = false;
success = true;
STXIdx = find(rx==uint8(2));
rxLen = length(rx);
for i=1:length(STXIdx) % 找到STX
    statusIdx = STXIdx(i) + 1;
    packetTypeIdx = STXIdx(i) + 2;
    lengthIdx = STXIdx(i) + 3;
    transNumIdx = STXIdx(i) + 4;
    pageIdx = STXIdx(i) + 5;
    maxPageIdx = STXIdx(i) + 6;
    msgBgnIdx = STXIdx(i) + 7;
    if (rxLen>=lengthIdx) && (rx(packetTypeIdx)==uint8(64)) % 为GENOUT包
        checksumIdx = transNumIdx + double(rx(lengthIdx));
        ETXIdx = checksumIdx + 1;
        if (rxLen>=ETXIdx) && (rx(ETXIdx)==uint8(3)) % 找到ETX
            if mod(sum(rx(statusIdx:(checksumIdx-1))), 256) == double(rx(checksumIdx)) % 校验和正确
                msg = rx(msgBgnIdx:(checksumIdx-1));
                packetItem = rx([statusIdx transNumIdx pageIdx maxPageIdx]);
                packetItem = repmat(packetItem(:), 1, checksumIdx-msgBgnIdx);
                hasValidPacket = true;
                rx(1:ETXIdx) = []; % 删掉已解析部分
                break;
            else
                success = false;
            end
        end
    end
end
end

function [msgParams, msg, packetItem, hasValidMsg] = parsemsg(msg, packetItem)
hasValidMsg = false;
msgLen = length(msg);
msgParams = struct();
for i=1:msgLen
    parsedMsgEndIdx = i;
    switch msg(i)
        case uint8(1) % TIME
            if (msgLen>=i+11) && (msg(i+1)==uint8(10))
                GPSTime = swapbytes(typecast(msg(i+2:i+5), 'uint32'));
                GPSWeekNum = swapbytes(typecast(msg(i+6:i+7), 'uint16'));
                numOfSVUsed = msg(i+8);
                posFlags1 = msg(i+9);
                posFlags2 = msg(i+10);
                initNum = msg(i+11);
                parsedMsgEndIdx = i + 11;
                msgParams.TIME = struct('GPSTime', GPSTime, 'GPSWeekNum', GPSWeekNum, 'numOfSVUsed', numOfSVUsed, 'posFlags1', posFlags1, 'posFlags2', posFlags2, 'initNum', initNum);
            end
        case uint8(2) % LLH
            if (msgLen>=i+25) && (msg(i+1)==uint8(24))
                latitude = swapbytes(typecast(msg(i+2:i+9), 'double'));
                longitude = swapbytes(typecast(msg(i+10:i+17), 'double'));
                height = swapbytes(typecast(msg(i+18:i+25), 'double'));
                parsedMsgEndIdx = i + 25;
                msgParams.LLH = struct('latitude',latitude, 'longitude',longitude,'height',height);
            end
        case uint8(8) % Velocity
            if (msgLen>=i+14) && (msg(i+1)==uint8(13))
                velFlags = msg(i+2);
                speed = swapbytes(typecast(msg(i+3:i+6), 'single'));
                heading = swapbytes(typecast(msg(i+7:i+10), 'single'));
                vertVel = swapbytes(typecast(msg(i+11:i+14), 'single'));
                parsedMsgEndIdx = i + 14;
                msgParams.Velocity = struct('speed',speed, 'heading',heading, 'vertVel',vertVel, 'velFlags', velFlags);
            end
        case uint8(9) % PDOP
            if (msgLen>=i+17) && (msg(i+1)==uint8(16))
                PDOP = swapbytes(typecast(msg(i+2:i+5), 'single'));
                HDOP = swapbytes(typecast(msg(i+6:i+9), 'single'));
                VDOP = swapbytes(typecast(msg(i+10:i+13), 'single'));
                TDOP = swapbytes(typecast(msg(i+14:i+17), 'single'));
                parsedMsgEndIdx = i + 17;
                msgParams.DOP = struct('PDOP',PDOP, 'HDOP',HDOP, 'VDOP',VDOP, 'TDOP',TDOP);
            end
        case uint8(12) % SIGMA
            if (msgLen>=i+39) && (msg(i+1)==uint8(38))
                posRMS = swapbytes(typecast(msg(i+2:i+5), 'single'));
                sigmaEast = swapbytes(typecast(msg(i+6:i+9), 'single'));
                sigmaNorth = swapbytes(typecast(msg(i+10:i+13), 'single'));
                covarEastNorth = swapbytes(typecast(msg(i+14:i+17), 'single'));
                sigmaUp = swapbytes(typecast(msg(i+18:i+21), 'single'));
                semiMajorAxis = swapbytes(typecast(msg(i+22:i+25), 'single'));
                semiMinorAxis = swapbytes(typecast(msg(i+26:i+29), 'single'));
                orientation = swapbytes(typecast(msg(i+30:i+33), 'single'));
                unitVar = swapbytes(typecast(msg(i+34:i+37), 'single'));
                numOfEpochs = swapbytes(typecast(msg(i+38:i+39), 'uint16'));
                parsedMsgEndIdx = i + 39;
                msgParams.SIGMA = struct('posRMS',posRMS, 'sigmaEast',sigmaEast, 'sigmaNorth',sigmaNorth, 'covarEastNorth',covarEastNorth, 'sigmaUp',sigmaUp, 'semiMajorAxis',semiMajorAxis, 'semiMinorAxis',semiMinorAxis, 'orientation',orientation, 'unitVar',unitVar, 'numOfEpochs',numOfEpochs);
            end
        case uint8(16) % UTC
            if (msgLen>=i+10) && (msg(i+1)==uint8(9))
                GPSTime = swapbytes(typecast(msg(i+2:i+5), 'uint32'));
                GPSWeekNum = swapbytes(typecast(msg(i+6:i+7), 'uint16'));
                UTCOffset = swapbytes(typecast(msg(i+8:i+9), 'uint16'));
                flags = msg(i+10);
                parsedMsgEndIdx = i + 10;
                msgParams.UTC = struct('GPSTime', GPSTime, 'GPSWeekNum', GPSWeekNum, 'UTCOffset', UTCOffset, 'flags', flags);
            end
        otherwise

    end
    if parsedMsgEndIdx ~= i
        msg(1:parsedMsgEndIdx) = []; % 删掉已解析部分
        packetItem(:, 1:parsedMsgEndIdx) = [];
        hasValidMsg = true;
        break;
    end
end
end